/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates.subsystems;

import edu.wpi.first.wpilibj.command.PIDSubsystem;
import edu.wpi.first.wpilibj.AnalogChannel;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.templates.RobotState;

/**
 *
 * @author MattKahn
 */
public class NewPIDLifterSubsystem extends PIDSubsystem {

    private static final int MinCount[/*
             * Lifter
             */][/*
             * Robot
             */] = {
        // Lifter minimum position (all down at mechanical stop) in sensor Counts
        // Kitty, Rook
        {157, 179}, // LF counts
        {321, 338}, // LR counts
        {30, 56}, // RF counts
        {91, 107} // RR counts
    };
    private static final int MaxCount[/*
             * Lifter
             */][/*
             * Robot
             */] = {
        // Lifter maximum position (all up at mechanical stop) in sensor Counts
        // Kitty, Rook
        {667, 653}, // LF counts
        {837, 825}, // LR counts
        {513, 500}, // RF counts
        {575, 559} // RR counts
    };
    private static final double TravelCm[/*
             * Lifter
             */][/*
             * Robot
             */] = {
        // Lifter travel from min position to max position in centimeters
        // Kitty, Rook
        {47.1, 44.2}, // LF cm
        {46.8, 44.3}, // LR cm
        {46.6, 43.8}, // RF cm
        {46.6, 44.3} // RR cm  
    };
    private static final int GroundCount[/*
             * Lifter
             */][/*
             * Robot
             */] = {
        // Lifter ground position (foot at ground level) in sensor Counts
        // Kitty, Rook
        {553, 539}, // LF counts
        {694, 716}, // LR counts
        {376, 394}, // RF counts
        {468, 456} // RR counts
    };
    // Distance of Drive Lowered (DL) postion below ground level
    private static final double DriveLoweredCm = 2.5;
    // Distance of Drive Raised (DR) postion above ground level
    private static final double DriveRaisedCm = 2.5;
    // Distance of High Clearance (HC) postion below upper mechanical stop
    private static final double HighClearanceCm = 2;
    // Distance of Stilt (ST) postion below ground level
    private static final double StiltCm = 28;
    // Distance entering swerve mode raises robot
    private static final double SwerveRiseCm = 3;

    public static class LifterPresetLocation {

        public int value;
        // Lifter PresetPositions
        protected static final int m_kHighClearance = 0;
        protected static final int m_kDriveNormalRaised = 1;
        protected static final int m_kDriveNormalLowered = 2;
        protected static final int m_kDriveSwerveRaised = 3;
        protected static final int m_kDriveSwerveLowered = 4;
        protected static final int m_kStilt = 5;
        public static final LifterPresetLocation kHighClearance = new LifterPresetLocation(m_kHighClearance);
        public static final LifterPresetLocation kDriveNormalRaised = new LifterPresetLocation(m_kDriveNormalRaised);
        public static final LifterPresetLocation kDriveNormalLowered = new LifterPresetLocation(m_kDriveNormalLowered);
        public static final LifterPresetLocation kDriveSwerveRaised = new LifterPresetLocation(m_kDriveSwerveRaised);
        public static final LifterPresetLocation kDriveSwerveLowered = new LifterPresetLocation(m_kDriveSwerveLowered);
        public static final LifterPresetLocation kStilt = new LifterPresetLocation(m_kStilt);

        private LifterPresetLocation(int position) {
            this.value = position;
        }
    }
// Lifter Location Enumeration

    public static class LifterID {

        public final int value;
        // Lifter ID enumeration constants
        protected static final int m_kLeftForeLifter = 0;
        protected static final int m_kLeftRearLifter = 1;
        protected static final int m_kRightForeLifter = 2;
        protected static final int m_kRightRearLifter = 3;
        public static final LifterID kLeftForeLifter = new LifterID(m_kLeftForeLifter);
        public static final LifterID kLeftRearLifter = new LifterID(m_kLeftRearLifter);
        public static final LifterID kRightForeLifter = new LifterID(m_kRightForeLifter);
        public static final LifterID kRightRearLifter = new LifterID(m_kRightRearLifter);

        private LifterID(int lifterID) {
            this.value = lifterID;
        }
    }
    private static final double Kp = 0.04;  // 0.02 is ok if a little slow...
    private static final double Ki = 0.0000005;
    private static final double Kd = 0.0;
    private LifterID m_Lifter;
    protected Victor actuator = null;
    protected AnalogChannel lifterPositionReading = null;
    private double lastEnableCommand;
    private boolean motorsEnabled = false;
    private static double m_kTimeToTraverseACount = 0.015;
    private static double m_kMinImpulseDuration = 0.5;
    private double m_controlImpulseDuration = 3.0; // seconds
    private String lifterName = "NA";
    private int lifterPosition[/*
             * Position
             */];

    // Initialize your subsystem here
    public NewPIDLifterSubsystem(final String name, final LifterID lifter, int pwmPort, int potPort) {
        super("PIDLifterSubsystem" + name, Kp, Ki, Kd);
        lifterName = name;
        System.out.println("Constructing NewPIDLifterSubsystem " + lifterName + " " + lifter.value + " PWM  " + pwmPort + " Pot " + potPort);
        m_Lifter = lifter;
        actuator = new Victor(pwmPort);
        lifterPositionReading = new AnalogChannel(potPort);

        lifterPosition = computeLifterPostions(lifter);

        System.out.println("NewPIDLifterSubsystem() Constants");
        System.out.println("  HC: " + lifterPosition[LifterPresetLocation.kHighClearance.value]);
//        SmartDashboard.putDouble(lifterName + "position - HC", lifterPosition[LifterPresetLocation.kHighClearance.value]);
        System.out.println("  DNR:" + lifterPosition[LifterPresetLocation.kDriveNormalRaised.value]);
//        SmartDashboard.putDouble(lifterName + "position - DNR", lifterPosition[LifterPresetLocation.kDriveNormalRaised.value]);
        System.out.println("  DNL:" + lifterPosition[LifterPresetLocation.kDriveNormalLowered.value]);
//        SmartDashboard.putDouble(lifterName + "position - DNL", lifterPosition[LifterPresetLocation.kDriveNormalLowered.value]);
        System.out.println("  DSR:" + lifterPosition[LifterPresetLocation.kDriveSwerveRaised.value]);
//        SmartDashboard.putDouble(lifterName + "position - DSR", lifterPosition[LifterPresetLocation.kDriveSwerveRaised.value]);
        System.out.println("  DSL:" + lifterPosition[LifterPresetLocation.kDriveSwerveLowered.value]);
//        SmartDashboard.putDouble(lifterName + "position - DSL", lifterPosition[LifterPresetLocation.kDriveSwerveLowered.value]);
        System.out.println("  ST: " + lifterPosition[LifterPresetLocation.kStilt.value]);
//        SmartDashboard.putDouble(lifterName + "position - ST", lifterPosition[LifterPresetLocation.kStilt.value]);

        setSetpointRange(lifterPosition[LifterPresetLocation.kHighClearance.value], lifterPosition[LifterPresetLocation.kStilt.value]);

        enable();

        setLifterPosition(lifterPosition[LifterPresetLocation.kHighClearance.value]);
    }

    private int[] computeLifterPostions(LifterID lifter) {
        int lifterIdx = lifter.value;

        RobotState robotState = RobotState.getInstance();
        int robotIdx = robotState.getRobot().value;

        double scale = ((double) MaxCount[lifterIdx][robotIdx] - (double) MinCount[lifterIdx][robotIdx]) / TravelCm[lifterIdx][robotIdx];

        int HC = (int) ((double) MaxCount[lifterIdx][robotIdx] - (HighClearanceCm * scale));
        int DNR = (int) ((double) GroundCount[lifterIdx][robotIdx] + (DriveRaisedCm * scale));
        int DNL = (int) ((double) GroundCount[lifterIdx][robotIdx] - (DriveLoweredCm * scale));
        int DSR = (int) ((double) GroundCount[lifterIdx][robotIdx] - (SwerveRiseCm * scale) + (DriveRaisedCm * scale));
        int DSL = (int) ((double) GroundCount[lifterIdx][robotIdx] - (SwerveRiseCm * scale) - (DriveLoweredCm * scale));
        int ST = (int) ((double) GroundCount[lifterIdx][robotIdx] - (StiltCm * scale));

        int positions[] = {HC, DNR, DNL, DSR, DSL, ST};

        return positions;
    }

    public void initDefaultCommand() {
        // Set the default command for a subsystem here.
    }

    public void setLifterPosition(int position) {
        //System.out.println("setLifterPosition(" + lifterName + ".value) = " + position);
        enable();
        m_controlImpulseDuration = m_kMinImpulseDuration + m_kTimeToTraverseACount * Math.abs(position - getLifterPosition());
        motorsEnabled = true;
        lastEnableCommand = Timer.getFPGATimestamp();
        //System.out.println("Enabling Lifter Motor " + lifterName + " @ " + lastEnableCommand + " # " + position);
        SmartDashboard.putDouble(lifterName + " command", position);
        setSetpoint(position);
    }

    public void setLifterPosition(LifterPresetLocation position) {
        int positionCnt = lifterPosition[position.value];
        switch (m_Lifter.value) {
            case LifterID.m_kLeftForeLifter:
                if(RobotState.RobotDirection.kForward == RobotState.getInstance().getDirection()) {
                    positionCnt += 0;
                } else if(RobotState.RobotDirection.kReverse == RobotState.getInstance().getDirection()) {
                    positionCnt += -20;
//                    positionCnt += 0;
                }
                break;
            case LifterID.m_kLeftRearLifter:
                if(RobotState.RobotDirection.kForward == RobotState.getInstance().getDirection()) {
//                    positionCnt += 0;
                    positionCnt += -20;
                } else if(RobotState.RobotDirection.kReverse == RobotState.getInstance().getDirection()) {
                    positionCnt += 0;
                }
                break;
            case LifterID.m_kRightForeLifter:
                if(RobotState.RobotDirection.kForward == RobotState.getInstance().getDirection()) {
                    positionCnt += 0;
                } else if(RobotState.RobotDirection.kReverse == RobotState.getInstance().getDirection()) {
                    positionCnt += 0;
                }
                break;
            case LifterID.m_kRightRearLifter:
                if(RobotState.RobotDirection.kForward == RobotState.getInstance().getDirection()) {
                    positionCnt += 0;
                } else if(RobotState.RobotDirection.kReverse == RobotState.getInstance().getDirection()) {
                    positionCnt += 0;
                }
                break;
        }

        //System.out.println("setLifterPosition(" + lifterName + ".preset) = " + position.value + ", " + lifterPosition[position.value]);
        enable();
        motorsEnabled = true;
        m_controlImpulseDuration = m_kMinImpulseDuration + m_kTimeToTraverseACount * Math.abs(lifterPosition[position.value] - getLifterPosition());
        lastEnableCommand = Timer.getFPGATimestamp();
        //System.out.println("Enabling Lifter Motor " + lifterName + " @ " + lastEnableCommand + " - " + lifterPosition[m_Lifter.value][position.value] );
        SmartDashboard.putDouble(lifterName + " command", positionCnt);
        setSetpoint(lifterPosition[position.value]);
    }

    public int getLifterPosition() {
        return (int) returnPIDInput();
    }
    double lastLifterTime = 0;

    protected double returnPIDInput() {
        SmartDashboard.putDouble(lifterName + " cycle", lastLifterTime - Timer.getFPGATimestamp());
        lastLifterTime = Timer.getFPGATimestamp();
        // Return your input value for the PID loop
        double pos = lifterPositionReading.getAverageValue();
        SmartDashboard.putDouble(lifterName + " current", pos);
        return pos;
    }

    protected void usePIDOutput(double output) {
        output = -output;
        // Use output to drive your system, like a motor
        SmartDashboard.putDouble(lifterName + " drive", output);
        SmartDashboard.putBoolean(lifterName + " enabled", motorsEnabled);

        //System.out.println(lifterName + " out = " + output);
        actuator.set(output);

        if ((motorsEnabled)
                && (Timer.getFPGATimestamp() > (lastEnableCommand + m_controlImpulseDuration))) {
            // NOTE: This order is important because disable()ing the PID controller stops immediately, it does
            //       not allow the function to finish
            //System.out.println("Disabling Lifter Motor " + lifterName + " " + m_Lifter + " @ " + lastEnableCommand);
            motorsEnabled = false;
            disable();
        }
    }
}
